/**
 * @file axis_ctrl.c
 * @brief X/Z轴通用控制实现（与硬件无关）
 */
#include "axis_ctrl.h"
#include <string.h>

// 单轴状态结构体（内部使用）
typedef struct {
    // 硬件接口
    axis_hw_ops hw;               // 硬件操作函数
    
    // 运动参数
    uint32_t pulse_interval;      // 脉冲间隔(ms)
    uint32_t pulse_cnt;           // 脉冲间隔计数器
    int32_t current_pos;          // 当前位置(脉冲)
    int32_t target_pos;           // 目标位置(脉冲)
    int32_t remain_steps;         // 剩余步数
    uint8_t dir;                  // 方向(0=反,1=正)
    
    // 原点回归参数
    axis_homing_cfg homing;       // 回归配置
    uint32_t homing_timer;        // 回归计时器(ms)
    uint32_t homing_step_cnt;     // 回归步数计数器
    
    // 状态
    axis_state state;             // 运行状态
     axis_state last_state;             // 运行状态
} axis_dev;

// 双轴设备实例（静态全局）
static axis_dev s_axis[AXIS_MAX];

// -------------------------- 私有函数：获取轴设备指针 --------------------------
static axis_dev* get_axis_dev(axis_id id) {
    if (id >= AXIS_MAX) return NULL;
    return &s_axis[id];
}

// -------------------------- 私有函数：输出单个脉冲 --------------------------
static void output_pulse(axis_dev* dev) {
    if (!dev || !dev->hw.set_pulse) return;
    dev->hw.set_pulse(1);         // 脉冲高电平
    // 软件延时（确保驱动器识别，实际应用中可替换为硬件延时）
    for (volatile uint32_t i = 0; i < 10; i++);
    dev->hw.set_pulse(0);         // 脉冲低电平
}

// -------------------------- 私有函数：更新原点回归流程 --------------------------
static void update_homing(axis_dev* dev) {
    if (!dev) return;
    
    switch (dev->state) {
        // 步骤1：检测是否已在原点
        case AXIS_STATE_HOMING_STEP1:
            dev->homing_timer++;
            if (dev->hw.read_origin && dev->hw.read_origin() == 1) {
                // 已在原点直接停止
                dev->dir = 0;
              //  dev->remain_steps = dev->homing.escape_steps;
              //  if (dev->hw.set_dir) dev->hw.set_dir(dev->dir);
                dev->current_pos = 0;
                dev->state = AXIS_STATE_HOMING_OK;
                dev->homing_step_cnt = 0;
                 break;
            } else {
                // 不在原点→直接进入快速寻原
                dev->state = AXIS_STATE_HOMING_STEP3;
                break;
            }
            // 超时判断
            if (dev->homing_timer > dev->homing.timeout_ms) {
                dev->state = AXIS_STATE_HOMING_ERR;
            }
            break;
        
        // 步骤2：脱离原点（等待剩余步数完成）
        case AXIS_STATE_HOMING_STEP2:
            if (dev->remain_steps == 0) {
                dev->state = AXIS_STATE_HOMING_STEP3;
                dev->homing_timer = 0;
            }
            // 超步数判断
            if (dev->homing_step_cnt > dev->homing.max_homing_steps) {
                dev->state = AXIS_STATE_HOMING_ERR;
            }
            break;
        
        // 步骤3：快速寻找原点（反方向）
        case AXIS_STATE_HOMING_STEP3:
            dev->homing_timer++;
            dev->homing_step_cnt++;
            
            if (dev->hw.read_origin && dev->hw.read_origin() == 1) {
                // 找到原点→先退步
                dev->dir = 1;
                dev->remain_steps = dev->homing.fine_adjust_steps;
                if (dev->hw.set_dir) dev->hw.set_dir(dev->dir);
                dev->state = AXIS_STATE_HOMING_STEP4;
                dev->homing_step_cnt = 0;
            }
            // 超时/超步数判断
            if (dev->homing_timer > dev->homing.timeout_ms || 
                dev->homing_step_cnt > dev->homing.max_homing_steps) {
                dev->state = AXIS_STATE_HOMING_ERR;
            }
            break;
        
        // 步骤4：精确定位（慢速靠近）
        case AXIS_STATE_HOMING_STEP4:
            dev->homing_step_cnt++;
            if (dev->remain_steps > 0) break;
            
            // 退步完成→慢速寻原
            dev->dir = 0;
            if (dev->hw.set_dir) dev->hw.set_dir(dev->dir);
            if (dev->hw.read_origin && dev->hw.read_origin() == 1) {
                // 定位完成→重置位置
                dev->current_pos = 0;
                dev->state = AXIS_STATE_HOMING_OK;
            }
            // 精调超步数判断
            if (dev->homing_step_cnt > (dev->homing.fine_adjust_steps * 2)) {
                dev->state = AXIS_STATE_HOMING_ERR;
            }
            break;
        
        default:
            break;
    }
}

// -------------------------- 私有函数：更新移动逻辑 --------------------------
static void update_movement(axis_dev* dev) {
    if (!dev) return;
    
    // 非移动状态不处理
    if (dev->state != AXIS_STATE_MOVING && 
        dev->state != AXIS_STATE_STEP_MOVE &&
        !(dev->state >= AXIS_STATE_HOMING_STEP1 && 
          dev->state <= AXIS_STATE_HOMING_STEP4)) {
        return;
    }
        //printf("update_movement: %d\n", dev->state); 
        // 累加脉冲计数器
        dev->pulse_cnt++;
    // 脉冲间隔未到
    if (dev->pulse_cnt < dev->pulse_interval) {
        return;
    }
    
    // 输出脉冲
    output_pulse(dev);
    dev->pulse_cnt = 0;  // 重置计数器
    
    // 更新位置和剩余步数
    dev->current_pos += (dev->dir == 1) ? 1 : -1;
    if (dev->remain_steps > 0) {
        dev->remain_steps--;
    }
    
    // 移动完成判断（普通移动/单步移动）
    if (dev->state == AXIS_STATE_MOVING && dev->remain_steps == 0) {
        dev->state = AXIS_STATE_IDLE;
    } else if (dev->state == AXIS_STATE_STEP_MOVE && dev->remain_steps == 0) {
        dev->state = AXIS_STATE_IDLE;
    }
}

// -------------------------- 公共接口实现 --------------------------
void axis_init(axis_id id, const axis_hw_ops* hw_ops, 
              const axis_homing_cfg* homing_cfg, uint32_t default_pulse_interval) {
    axis_dev* dev = get_axis_dev(id);
    if (!dev || !hw_ops || !homing_cfg) return;
    
    // 初始化硬件接口
    memcpy(&dev->hw, hw_ops, sizeof(axis_hw_ops));
    
    // 初始化回归参数
    memcpy(&dev->homing, homing_cfg, sizeof(axis_homing_cfg));
    //打印出来回归参数
    printf("axis_init: homing_cfg = {escape_steps=%d, fine_adjust_steps=%d, max_homing_steps=%d, timeout_ms=%d}\n",
           dev->homing.escape_steps, dev->homing.fine_adjust_steps, dev->homing.max_homing_steps, dev->homing.timeout_ms);
 



    // 初始化运动参数
    dev->pulse_interval = (default_pulse_interval < 1) ? 1 : default_pulse_interval;
    dev->pulse_cnt = 0;
    dev->current_pos = 0;
    dev->target_pos = 0;
    dev->remain_steps = 0;
    dev->dir = 0;
    
    // 初始化状态
    dev->state = AXIS_STATE_IDLE;
    dev->homing_timer = 0;
    dev->homing_step_cnt = 0;

    //打印出来回归参数

}

void axis_set_pulse_interval(axis_id id, uint32_t interval_ms) {
    axis_dev* dev = get_axis_dev(id);
    if (dev) {
        dev->pulse_interval = (interval_ms < 1) ? 1 : interval_ms;
    }
}

bool axis_start_homing(axis_id id) {
   if(axis_is_move_complete(id)==false)
    {
         return false;
    }
    axis_dev* dev = get_axis_dev(id);
    // 启动回归流程
    dev->state = AXIS_STATE_HOMING_STEP1;
    dev->homing_timer = 0;
    dev->homing_step_cnt = 0;
    dev->remain_steps = 0;
    printf("axis_start_homing :%d\n", dev->state);
    return true;
}

bool axis_move_to(axis_id id, int32_t target_pos) {
   if(axis_is_move_complete(id)==false)
    {
         return false;
    }
    axis_dev* dev = get_axis_dev(id);
    // 计算步数和方向
    dev->target_pos = target_pos;
    dev->remain_steps = target_pos - dev->current_pos;
    
    if (dev->remain_steps > 0) {
        dev->dir = 1;
        if (dev->hw.set_dir) dev->hw.set_dir(1);
    } else if (dev->remain_steps < 0) {
        dev->dir = 0;
        if (dev->hw.set_dir) dev->hw.set_dir(0);
        dev->remain_steps = -dev->remain_steps;  // 转为正数
    } else {
        return true;  // 已在目标位置
    }
    
    dev->state = AXIS_STATE_MOVING;
    return true;
}

void axis_move_step(axis_id id, uint8_t dir) {
    
    if(axis_is_move_complete(id)==false)
    {
         return ;
    }
    axis_dev* dev = get_axis_dev(id);
 
    
    
    dev->dir = dir;
    dev->remain_steps = 1;
    dev->state = AXIS_STATE_STEP_MOVE;
    if (dev->hw.set_dir) {
        dev->hw.set_dir(dir);
    }
}
/**
 * @brief 前进指定步数（基于当前位置向正方向移动）
 */
bool axis_forward_steps(axis_id id, uint32_t steps) {
    // 参数校验：步数必须>0
    if (steps == 0) {
        return false;
    }
    
    axis_dev* dev = get_axis_dev(id);
    if (!dev || dev->state != AXIS_STATE_IDLE) {
        return false; // 轴正忙或无效
    }
    
    // 计算目标位置（当前位置 + 步数）
    int32_t target_pos = dev->current_pos + (int32_t)steps;
    return axis_move_to(id, target_pos); // 复用已有移动接口
}

/**
 * @brief 后退指定步数（基于当前位置向反方向移动）
 */
bool axis_backward_steps(axis_id id, uint32_t steps) {
    // 参数校验：步数必须>0
    if (steps == 0) {
        return false;
    }
    
    axis_dev* dev = get_axis_dev(id);
    if (!dev || dev->state != AXIS_STATE_IDLE) {
        return false; // 轴正忙或无效
    }
    
    // 计算目标位置（当前位置 - 步数）
    int32_t target_pos = dev->current_pos - (int32_t)steps;
    return axis_move_to(id, target_pos); // 复用已有移动接口
}
void axis_stop(axis_id id) {
    axis_dev* dev = get_axis_dev(id);
    if (!dev) return;
    
    dev->state = AXIS_STATE_IDLE;
    dev->remain_steps = 0;
    dev->pulse_cnt = 0;
    if (dev->hw.set_pulse) {
        dev->hw.set_pulse(0);  // 确保脉冲低电平
    }
}

axis_state axis_get_state(axis_id id) {
    axis_dev* dev = get_axis_dev(id);
    return (dev) ? dev->state : AXIS_STATE_ERROR;
}
/**
 * @brief 判断移动是否完成
 */
bool axis_is_move_complete(axis_id id) {
    axis_state state = axis_get_state(id);
    // 完成状态：空闲、回归完成
    return (state == AXIS_STATE_IDLE || 
            state == AXIS_STATE_HOMING_OK);
}
/**
 * @brief 判断轴是否在原点（直接读取传感器状态）
 */
bool axis_is_at_origin(axis_id id) {
    axis_dev* dev = get_axis_dev(id);
    if (!dev || !dev->hw.read_origin) {
        return false; // 设备未初始化或无原点传感器接口
    }
    
    // 直接调用硬件层的原点传感器读取函数
    // 注意：返回值与传感器硬件逻辑一致（1=检测到原点）
    return (dev->hw.read_origin() == 1);
}
int32_t axis_get_current_pos(axis_id id) {
    axis_dev* dev = get_axis_dev(id);
    return (dev) ? dev->current_pos : 0;
}

void axis_1ms_tick(void) {
    // 遍历所有轴，更新状态（支持多轴扩展）
    for (axis_id id = AXIS_X; id < AXIS_MAX; id++)
     {
        axis_dev* dev = get_axis_dev(id);
        if (!dev) continue;
        //状态变化打印出来
        if (dev->state != dev->last_state) {
          //  printf("axis_1ms_tick: id=%d, state=%d\n", id, dev->state);
            dev->last_state = dev->state;
        }
    
        
        // 处理原点回归
        if (dev->state >= AXIS_STATE_HOMING_STEP1 && 
            dev->state <= AXIS_STATE_HOMING_STEP4) {
            update_homing(dev);
        }
        
        // 处理移动（包括回归过程中的移动）
        update_movement(dev);
    }
}